/**
* @file main.c,使用外部中断获取编码器计数值
FOC控制

* @date 2023年7月31日
*/
#include "hardware.h"

float target;

void commander_run(void);

int main(void)
{

    hardware_Init();
	
	uart_init(115200);

	I2C_Init_();               //AS5600
	printf("AS5600\r\n");

	TIM2_PWM_Init();
	TIM3_1ms_Init();           //interrupt per 1ms
	
	delay_ms(1000);            //Wait for the system to stabilize
	MagneticSensor_Init();     //AS5600 or TLE5012B
	
	voltage_power_supply=12;   //V
	voltage_limit=4;           //V，最大值需小于12/1.732=6.9
	velocity_limit=20;         //rad/s angleOpenloop() and PID_angle() use it
	voltage_sensor_align=3;    //V     alignSensor() and driverAlign() use it，大功率电机0.5-1，小功率电机2-3
	torque_controller=Type_voltage;  //当前只有电压模式
	controller=Type_angle;  //Type_angle; 
	target=0;
	
	Motor_init();
	Motor_initFOC();
	PID_init();
  printf("Motor ready.\r\n");
	
	systick_CountMode();   //不能再调用delay_us()和delay_ms()函数
	
	while (1)
	{
		move(target);
		loopFOC();
		commander_run();
	}
}

void commander_run(void)
{
	if((USART_RX_STA&0x8000)!=0)
	{
		switch(USART_RX_BUF[0]) //串口接收字节
		{
			case 'H':
				printf("Hello World!\r\n");
				break;
			case 'T':   //T6.28
				target=atof((const char *)(USART_RX_BUF+1));//将字串转换成浮点型数
				printf("RX=%.4f\r\n", target);
				break;
			case 'D':   //D
				M1_Disable;
				printf("OK!\r\n");
				break;
			case 'E':   //E
				M1_Enable;
				printf("OK!\r\n");
				break;
		}
		USART_RX_STA=0;
	}
}